#include <Arduino.h>
#include "BTSerial.h"
#include "BluetoothSerial.h"


#include "movement.h"


#include "hexapod.h"
extern BluetoothSerial SerialBT;
#define WIDTH 9
#define HEIGHT 18
#define L_W WIDTH
#define L_H 1
#define B_W 2
#define B_H 2
#define T_W 5
#define T_H 2

#define L_X(g)  0
#define L_Y(g)  (L_H+T_H)*g
#define Bl_X(g)  0
#define Bl_Y(g)  (L_H+T_H)*g+L_H
#define T_X(g)  B_W
#define T_Y(g)  (L_H+T_H)*g+L_H
#define Br_X(g)  B_W+T_W
#define Br_Y(g)  (L_H+T_H)*g+L_H
void setting_setup(int btn_id);
//---------------------------------------------------------------------------
// UI Group: Adjust setting
/*
static Label label1("Select leg", L_X(0), L_Y(0), L_W, L_H);
static Label text1("", T_X(0), T_Y(0), T_W, T_H, RC_PINK);
static Button button1l(10, "<", Bl_X(0), Bl_Y(0), B_W, B_H, RC_BLUE);
static Button button1r(11, ">", Br_X(0), Br_Y(0), B_W, B_H, RC_BLUE);

static Label label2("Select joint", L_X(1), L_Y(1), L_W, L_H);
static Label text2("", T_X(1), T_Y(1), T_W, T_H, RC_PINK);
static Button button2l(20, "<", Bl_X(1), Bl_Y(1), B_W, B_H, RC_BLUE);
static Button button2r(21, ">", Br_X(1), Br_Y(1), B_W, B_H, RC_BLUE);

static Label label3("Servo offset", L_X(2), L_Y(2), L_W, L_H);
static Label text3("0", T_X(2), T_Y(2), T_W, T_H, RC_PINK);
static Button button3l(30, "-", Bl_X(2), Bl_Y(2), B_W, B_H, RC_BLUE);
static Button button3r(31, "+", Br_X(2), Br_Y(2), B_W, B_H, RC_BLUE);

static Label label4("Servo scale", L_X(3), L_Y(3), L_W, L_H);
static Label text4("0", T_X(3), T_Y(3), T_W, T_H, RC_PINK);
static Button button4l(40, "-", Bl_X(3), Bl_Y(3), B_W, B_H, RC_BLUE);
static Button button4r(41, "+", Br_X(3), Br_Y(3), B_W, B_H, RC_BLUE);

static Button buttonSave(60, "Save", 0, Bl_Y(5), WIDTH, B_H, RC_GREEN);

static ButtonGroup btnGroup;

//---------------------------------------------------------------------------
// UI Group: Test

static Label label5("Servo test", L_X(4), L_Y(4), L_W, L_H);
static Button button5l(-45, "-45°", 0, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
static Button button5c(0, "0°", WIDTH/3, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
static Button button5r(45, "+45°", WIDTH*2/3, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);

static ButtonGroup btnGroupTest;
*/
//---------------------------------------------------------------------------

const static String _leg_table[] = {
  "front right",
  "right",
  "rear right",
  "rear left",
  "left",
  "front left",
};

const static String _joint_table[] = {
  "body",
  "thigh",
  "foot",
};

static int _cur_leg = 0;
static int _cur_joint = 0;
static int _cur_offset = 0;
static int _cur_scale = 0;
static int _test_angle[3*6] = {0};
/*
void update_text(void) {
  text1.updateText(_leg_table[_cur_leg]);
  text2.updateText(_joint_table[_cur_joint]);
  text3.updateText(String(_cur_offset));
  text4.updateText(String(1+0.01*_cur_scale));
}
*/
static uint8_t led_blink_state = 0;
void setting_loop(){
  int i,b;
  for(i=0;i<6;i++)
  for(b=0;b<3;b++){
    hexapod::Hexapod.calibrationGet(i, b, _cur_offset, _cur_scale);
	SerialBT.printf("leg%d,num%d,offset:%d,scale:%d,_",i,b,_cur_offset,_cur_scale);
  }

	while(1){
	auto flag =check_serial_BT(); 
	if(flag=='r') return;
	setting_setup(flag);
	 }

}
void setting_setup(int btn_id) {

//  digitalWrite(LED_BUILTIN, HIGH);
auto mode = hexapod::MOVEMENT_STANDBY;

if(btn_id!=0){
    Serial.print("button id: ");
    Serial.print(String(btn_id));
    Serial.println(" is triggered");
}
    int g = btn_id / 10;
    int lr = btn_id % 10;

    switch(g) {
    case 4:
      _cur_leg = (_cur_leg + 6 + (lr ? 1 : -1)) % 6;
      hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
     // hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
	  hexapod::Hexapod.processMovement(mode, 5);
	  Serial.printf("_cur_leg is:%d\r\n",_cur_leg);
    SerialBT.printf("leg%d,num%d,offset:%d,scale:%d,_",_cur_leg,_cur_joint,_cur_offset,_cur_scale);
      break;
    case 5:
      _cur_joint = (_cur_joint + 3 + (lr ? 1 : -1)) % 3;
      hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
     // hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
     hexapod::Hexapod.processMovement(mode, 5);
	  Serial.printf("_cur_joint is:%d\r\n",_cur_joint);
    SerialBT.printf("leg%d,num%d,offset:%d,scale:%d,_",_cur_leg,_cur_joint,_cur_offset,_cur_scale);
      break;
    case 6:
      _cur_offset += (lr ? 10 : -10);
      hexapod::Hexapod.calibrationSet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
      
      hexapod::Hexapod.processMovement(mode, 5);
		Serial.printf("_cur_offset is:%d\r\n",_cur_offset);
    SerialBT.printf("leg%d,num%d,offset:%d,scale:%d,_",_cur_leg,_cur_joint,_cur_offset,_cur_scale);
      break;
    case 7:
      _cur_scale += (lr ? 10 : -10);
      hexapod::Hexapod.calibrationSet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
      
      hexapod::Hexapod.processMovement(mode, 5);
	Serial.printf("_cur_scale is:%d\r\n",_cur_scale);
  SerialBT.printf("leg%d,num%d,offset:%d,scale:%d,_",_cur_leg,_cur_joint,_cur_offset,_cur_scale);
      break;
    case 8:
      hexapod::Hexapod.calibrationSave();
      SerialBT.printf("entern!!");
    }
  //  update_text();
  }





